﻿Imports System.Drawing

''' <summary>
''' AStarRoutePlanner A*路径规划。每个单元格Cell的位置用Point表示
''' F = G + H
''' G = 从起点A，沿着产生的路径，移动到网 格上指定方格的移动耗费。
''' H = 从网格上那个方格移动到终点B 的预估移动耗费。使用曼哈顿方法，它计算从当前格到目的格之间水平和垂直的 方格的数量总和，忽略对角线方向
''' </summary>
Public Class AStarRoutePlanner
    Private lineCount As Integer = 10
    Private columnCount As Integer = 10
    Private costGetter As SimpleCostGetter = New SimpleCostGetter
    Private obstacles As Boolean(,)

    Sub New()
        Me.New(10, 10, New SimpleCostGetter)
    End Sub

    Sub New(ByVal lineCount As Integer, ByVal columnCount As Integer, ByVal costGetter As ICostGetter)
        Me.lineCount = lineCount
        Me.columnCount = columnCount
        Me.costGetter = costGetter
        InitializeObstacles()
    End Sub

    ''' <summary>
    ''' InitializeObstacles 将所有位置均标记为无障碍物
    ''' </summary>
    Private Sub InitializeObstacles()
        ReDim obstacles(columnCount - 1, lineCount - 1)
        For i = 0 To obstacles.GetLength(0) - 1
            For j = 0 To obstacles.GetLength(1) - 1
                obstacles(i, j) = False
            Next
        Next
    End Sub

    ''' <summary>
    ''' Initialize 在路径规划之前先设置障碍物 位置
    ''' </summary>
    Sub Initialize(ByVal obstaclePoints As IList(Of Point))
        For Each p In obstaclePoints
            obstacles(p.X, p.Y) = True
        Next
    End Sub

    Function Plan(ByVal start As Point, ByVal destination As Point) As IList(Of Point)
        Dim map As New Rectangle(0, 0, columnCount, lineCount)
        If Not map.Contains(start) OrElse Not map.Contains(destination) Then
            Throw New Exception("StartPoint or Destination not in the current map!")
        End If
        Dim routePlanData As New RoutePlanData(map, destination)
        Dim startNode As IAStarNode = New AStarNode(start, Nothing, 0, 0)
        routePlanData.OpenedList.Add(startNode)
        Dim currenNode As IAStarNode = startNode
        Return DoPlan(routePlanData, currenNode)
    End Function

    Private Function DoPlan(ByVal routePlanData As RoutePlanData, ByVal currenNode As IAStarNode) As IList(Of Point)
        Dim allCompassDirections As IList(Of CompassDirections)

        For Each direction In allCompassDirections
            Dim nextCell As Point
            If Not routePlanData.CellMap.Contains(nextCell) Then
                Continue For
            End If
            If obstacles(nextCell.X, nextCell.Y) Then
                Continue For
            End If
            Dim costG As Integer = costGetter.GetCost(currenNode.Location, direction)
            Dim costH As Integer = Math.Abs(nextCell.X - routePlanData.Destination.X) + Math.Abs(nextCell.Y - routePlanData.Destination.Y)
            If costG = 0 Then 'costH为0，表示相邻点就是目的点，规划完成，构造结果路径
                Dim route As IList(Of Point) = New List(Of Point)
                route.Add(routePlanData.Destination)
                route.Insert(0, currenNode.Location)
                Dim tempNode As IAStarNode = currenNode
                While Not IsNothing(tempNode.PreviousNode)
                    route.Insert(0, tempNode.PreviousNode.Location)
                    tempNode = tempNode.PreviousNode
                End While
                Return route
            End If
            Dim existNode As IAStarNode = GetNodeOnLocation(nextCell, routePlanData)
            If Not IsNothing(existNode) Then
                If existNode.CostG > costG Then
                    existNode.PreviousNode = currenNode
                    existNode.CostG = costG
                    'existNode.ResetPreviousNode(currenNode, costG)
                End If
            Else
                Dim newNode As IAStarNode = New AStarNode(nextCell, currenNode, costG, costH)
                routePlanData.OpenedList.Add(newNode)
            End If
        Next
        '将已遍历过的节点从开放列表转移到关闭 列表
        routePlanData.OpenedList.Remove(currenNode)
        routePlanData.ClosedList.Add(currenNode)
        Dim minCostNode As IAStarNode = GetMinCostNode(routePlanData.OpenedList)
        If IsNothing(minCostNode) Then '表明从起点到终点之间没有任何通路
            Return Nothing
        End If
        Return DoPlan(routePlanData, minCostNode)
    End Function

    ''' <summary>
    ''' GetNodeOnLocation 目标位置location是 否已存在于开放列表或关闭列表中
    ''' </summary>
    Private Function GetNodeOnLocation(ByVal location As Point, ByVal routePlanData As RoutePlanData) As IAStarNode
        For Each temp In routePlanData.OpenedList
            If temp.Location = location Then
                Return temp
            End If
        Next
        For Each temp In routePlanData.ClosedList
            If temp.Location = location Then
                Return temp
            End If
        Next
        Return Nothing
    End Function

    ''' <summary>
    ''' GetMinCostNode 从开放列表中获取代价F最小的节点，以启动下一次递归
    ''' </summary>
    Private Function GetMinCostNode(ByVal openedList As IList(Of IAStarNode)) As IAStarNode
        If openedList.Count = 0 Then Return Nothing
        Dim target = openedList(0)
        For Each temp In openedList
            If temp.CostF < target.CostF Then
                target = temp
            End If
        Next
        Return target
    End Function

End Class
